#include "protocolServer.h"
#include "liveMediaRead.h"

#include <string.h>
#include <linux/can/raw.h>


void ControlInfoDebug(ControlInfo &ctlInfo){

}

protocolServer::protocolServer():
    uartFD(-1),canFD(-1),
    comDataIndex(0),canDataIndex(0)
{
    memset(bufCanData,0,PROTOCOL_DATA_BUFFER_SIZE);
    memset(bufComData,0,PROTOCOL_DATA_BUFFER_SIZE);
    memset(&ctlStaInfo,0,sizeof(ControlInfo));

    ctlStaInfo.detectMode = 0xFF;   // 默认为待机状态，不开检测

    boardStat.selfTestStat = 0x00;
    boardStat.sysStat = 0x00;
    boardStat.xIndex = 0x00;


}

protocolServer::~protocolServer(){
    
}

 

int protocolServer::cmdParser(ControlInfo &ctlInfo)
{
   
    INFO_PRT("ctlStaInfo.detectMode %x\n",ctlStaInfo.detectMode);

    // 跟踪模式下，检测是否解锁
    if(procManager->currentAlgorMode() == 1 && ctlInfo.targetLock == 0x00){   
        procManager->switchIdleMode();
        return 0;                
    }

    // 非跟踪状态，存在目标。锁定目标，开启跟踪
    if(procManager->currentAlgorMode() != 1 && ctlInfo.targetLock == 0x11 && osd_rect.detect.empty() == false){

        auto rt = osd_rect.detect[0];
        if (rt.w != 0 && rt.h != 0)
        {
            // 1920*1080 系数
            constexpr double xsca = VPSS_CHN2_IMAGE_WIDTH * 1.0 / VI_INPUT_WIDTH;
            constexpr double ysca = VPSS_CHN2_IMAGE_HEIGHT * 1.0 / VI_INPUT_HEIGHT;

            DetectRect rect(osd_rect.detect[0].x * xsca, osd_rect.detect[0].y * ysca, osd_rect.detect[0].w * xsca, osd_rect.detect[0].h * ysca);
            procManager->switchTrackMode(rect);
            return 0;
        }
    }


    // 判断工作模式是否产生变化 0x00自动 0x11手动。现默认手动，不运行识别算法
    if(ctlStaInfo.detectMode != ctlInfo.detectMode){

        //24/3/12 手动检测模式，由于没有相应的算法，目前还是直接使用全局检测，不做任何处理
        procManager->switchDetectMode();

        // if (ctlInfo.detectMode == 0x11) 
        // {
        //     // 3840*2160 ->1280*1024  
        //     double xsca = 0.33,ysca = 0.5;
        //     // 给出的为左顶点坐标
        //     uint16_t y = ctlInfo.manualYawlos*ysca;
        //     uint16_t x = ctlInfo.manualPitchlos*xsca;

        //     uint16_t w = ctlInfo.targetWidth*ysca;
        //     uint16_t h = ctlInfo.targetHeight*xsca;

        //     INFO_PRT("manual mode x %d,y %d,w %d,h %d\n",x,y,w,h);
            

        //     DetectRect tRect(x,y,w,h);

        //     //手动跟踪模式。手动给出目标，直接进入跟踪模式
        //     procManager->switchTrackMode(tRect);
        //     ctlStaInfo = ctlInfo;
        //     return 0;
        // }else{
        //     //自动模式搜索模式
        //     procManager->switchDetectMode();   
        // }
    }
    
    // 目标锁定与解锁
    // if (ctlInfo.targetLock != ctlStaInfo.targetLock)
    // {
    //     if (ctlInfo.targetLock == 0x11 && ctlInfo.detectMode == 0x00)//锁定目标，转为跟踪
    //     {

    //         // boardStat中存储的是顺时针旋转90度存储的图像，需要反算回来（2160*3840->1280*1024）

    //         // 转换到3840*2160坐标系下
    //         INFO_PRT("src 2160*3840 x %d,y %d, w %d, h %d\n",
    //                     boardStat.Yawlos,boardStat.Pitchlos,boardStat.targetWidth,boardStat.targetHeight);

    //         uint16_t x = boardStat.Pitchlos; 
    //         uint16_t y = (VI_INPUT_HEIGHT - boardStat.Yawlos);

    //         uint16_t w = boardStat.targetHeight;
    //         uint16_t h = boardStat.targetWidth;

    //         INFO_PRT("to 3840*2160 x %d,y %d, w %d, h %d\n",x,y,w,h);

             
    //         DetectRect tRect;
    //         tRect.point[0][0] = x;
    //         tRect.point[0][1] = y ;

    //         tRect.point[1][0] = x + w;
    //         tRect.point[1][1] = y ;

    //         tRect.point[2][0] = x ;
    //         tRect.point[2][1] = y + h;

    //         tRect.point[3][0] = x + w;
    //         tRect.point[3][1] = y + h;
    //         procManager->switchTrackMode(tRect);
    //     }else{
    //         //解锁，转为搜索模式
    //         procManager->switchDetectMode();
    //     }
    // }

    // 检测阈值 todo
    
    // 曝光控制 todo


    ctlStaInfo = ctlInfo;

    return 0;
}

void protocolServer::ctlInfoSave(const uint8_t* data,ControlInfo& ctlInfo){
    int offset = 0;
    ctlInfo.fixHead = ((uint16_t)data[offset] << 8) + (uint16_t)data[offset+1];
    

    ctlInfo.frameLength = PROTOCOL_CMD_STATIC_LENGTH;
    offset += 3;


    ctlInfo.frame_id = ((uint16_t)data[offset + 1] << 8) + (uint16_t)data[offset];
    offset +=2;

    ctlInfo.launchFlag = data[offset++];
    ctlInfo.targetType = data[offset++];
    ctlInfo.targetLock = data[offset++];
    ctlInfo.targetModify = data[offset++];
    ctlInfo.detectMode = data[offset++];

    ctlInfo.autoPitchlos = ((uint16_t)data[offset + 1] << 8) + (uint16_t)data[offset];
    offset +=2;

    ctlInfo.autoYawlos = ((uint16_t)data[offset + 1] << 8) + (uint16_t)data[offset];
    offset +=2;

    ctlInfo.manualPitchlos = ((uint16_t)data[offset + 1] << 8) + (uint16_t)data[offset];
    offset +=2;

    ctlInfo.manualYawlos = ((uint16_t)data[offset + 1] << 8) + (uint16_t)data[offset];
    offset +=2;

    ctlInfo.targetWidth = data[offset++];
    ctlInfo.targetHeight = data[offset++];

    ctlInfo.detectThreshold = data[offset++];
    ctlInfo.exposureMode = data[offset++];

    ctlInfo.resver_1 = data[offset++];

    ctlInfo.pitchSightAngle = ((uint16_t)data[offset + 1] << 8) + (uint16_t)data[offset];
    offset +=2;

    ctlInfo.yawLineSightAngle = ((uint16_t)data[offset + 1] << 8) + (uint16_t)data[offset];
    offset +=2;

    ctlInfo.pointSwitch = data[offset++];
    ctlInfo.lossView = data[offset++];
    

}

int protocolServer::realParser(uint8_t *data, int length)
{
    
    int offset = 0;
    while (length - offset >= PROTOCOL_DATA_STATIC_LENGTH)
    {
        //INFO_PRT("length %d need parse data[0-2] %x,%x,%x\n",length - offset,data[offset],data[offset+1],data[offset+2]);
        if (data[offset] == 0xAA && data[offset + 1] == 0x55 && data[offset + 2] == PROTOCOL_CMD_STATIC_LENGTH)
        {
            offset += 3;
            // 检查数据是否正确
            uint32_t sum = PROTOCOL_CMD_STATIC_LENGTH;
            int i = 0 ;
            for(;i<PROTOCOL_CMD_STATIC_LENGTH;i++){
                sum += data[offset + i];
            }

            if((uint8_t)(sum&0xFF) != data[offset + i]){
                ERRO_PRT("checksum error, cmd %x, recv %x\n",data[offset + i],sum&0xFF);
                offset += PROTOCOL_DATA_STATIC_LENGTH - 3;
                continue;
            }

        
            ControlInfo ctlInfo = {};
            //memcpy((void *)&ctlInfo, data + offset - 3, PROTOCOL_DATA_STATIC_LENGTH);
            ctlInfoSave(data + offset - 3,ctlInfo);

            cmdParser(ctlInfo);
            
            offset += PROTOCOL_DATA_STATIC_LENGTH - 3;
        }
        else
        {
            offset++;
        }
    }

    return offset;
}

int protocolServer::protocalParse(int fd,const char* data,int length,std::string& repStr){

    if (length < 0)
    {
        ERRO_PRT("error fd select\n");
        if (fd == uartFD)
        {
            comTool->serialClose();
            uartFD = -1;
        }
        return -1;
    }
    //INFO_PRT("serial fd %d,com recv len %d\n",fd,length);

    // 通信串口接收到的数据
    if (fd == uartFD)
    {
        // for (int i(0); i < length; i++)
        // {
        //     printf("%02x ", data[i]);
        // }
        // printf("\n");

        // 拷贝数据到缓冲区进行解析，处理数据断包与粘包问题
        memcpy(bufComData + comDataIndex, data, length);

        int usedSize = realParser(bufComData, comDataIndex + length);
        if (usedSize > 0)
        {
            comDataIndex = comDataIndex + length - usedSize;
            memmove(bufComData, bufComData + usedSize, comDataIndex);
        }
    }
    else  if(fd == canFD)   // can总线接收的数据
    {   
        struct can_frame *cf = (struct can_frame *)data;
        INFO_PRT("can id %d,len %02d\n",cf->can_id,cf->can_dlc);
        for (int i(0); i < cf->can_dlc; i++)
        {
            printf("%02x ", cf->data[i]);
        }
        printf("\n");

        // 拷贝数据到缓冲区进行解析，处理数据断包与粘包问题
        
        memcpy(bufCanData + canDataIndex, cf->data, cf->can_dlc);
        canDataIndex += cf->can_dlc;

        int usedSize = realParser(bufCanData, canDataIndex);
        if (usedSize > 0)
        {
            //INFO_PRT("canData Used %d,all %d\n",usedSize,canDataIndex);
            canDataIndex -= usedSize;
            memmove(bufCanData, bufCanData + usedSize, canDataIndex);
        }else{
            //INFO_PRT("canDataIndex %d\n",canDataIndex);
        }
        INFO_PRT("canDataIndex length %d\n",canDataIndex);
    }else{
        ERRO_PRT("error unknow fd recv %d\n",fd);
    }

    return length;
}

int protocolServer::serverInit(){

    // 串口通信初始化
    comTool.reset(new serialTool(this));
    std::string comDevName(SERIAL_SERVER_COM);
    uartFD = comTool->serialToolInit(comDevName,SERIAL_SERVER_COM_SPEED);
    if(uartFD < 0){
        ERRO_PRT("serialToolInit errro %d\n",uartFD);
        return -1;
    }

    //can总线通信初始化
    canTool.reset(new canbusTool(this,CANBUS_DEVICE_CANID));
    std::string canDevName(CANBUS_DEVICE_NAME);
    canFD = canTool->canbusInit(canDevName);
    if(canFD < 0){
        ERRO_PRT("canbusTool errro %d\n",uartFD);
        return -1;
    }


    // 消息同步使用
    cond.reset(new notifyCond<image_detect_info>);
    
    // 算法管理
    procManager.reset(new ImgProcManager(cond));
    int ret = procManager->procSysInit();
    if(ret < 0){
        ERRO_PRT("error ImgProcManager procSysInit error\n");
        return -1;
    }

    // 运行算法
    procManager->ImgAlgorProcRun();
    

    hThrStatus.reset(new ThreadHelper(std::bind(&protocolServer::algorResultUpdate,this)));

    INFO_PRT("img algor start detect\n");   
    return 0;
} 

int protocolServer::sendTest(){
    struct can_frame cf = {};
    cf.can_id = 0x12;
    cf.can_dlc = 4;
    cf.data[0] = 00;cf.data[1] = 01;cf.data[2] = 02;cf.data[3] = 03;

    char str[4] = {0x00,0x01,0x02,0x03};
    comTool->serialSend(str,4);
    canTool->canbusSend((uint8_t*)&cf,sizeof(cf));
    return 0;
}



int protocolServer::algorResultUpdate(){
   // INFO_PRT("start fdsst track++++++++++\n");
    // static int i = 0;
    // if (i++ == 30)
    // {
    //     // 1920*1080 系数 
    //     double xsca = 0.67,ysca = 0.94;
    //     // 测试输入为左顶点坐标
    //     INFO_PRT("start fdsst track++++++++++\n");
    //     int x = 910 *xsca , y = 510 * ysca;
    //     DetectRect tRect(x,y,100,60);

    //     // 手动模式。手动给出目标，直接进入跟踪模式
    //     procManager->switchTrackMode(tRect);
    //     //procManager->switchDetectMode();
    // }

    osd_rect.detect.clear();
    int ret = cond->conditionWait(std::chrono::milliseconds(300),osd_rect);
    if(ret == -1){ 
        return 0;
    }
 
    recognized_target_update(osd_rect);

    

    {
        std::lock_guard<std::mutex> lock(mutex_ctlInfo);
        boardStat.frameID = ctlStaInfo.frame_id;
        boardStat.launchFlag = ctlStaInfo.launchFlag;
        boardStat.targetLock = ctlStaInfo.targetLock;
        boardStat.sendpitchSightAngle = ctlStaInfo.pitchSightAngle;
        boardStat.sendyawLineSightAngle = ctlStaInfo.yawLineSightAngle;
        boardStat.detectMode = ctlStaInfo.detectMode;
        boardStat.outofView = ctlStaInfo.lossView;
        boardStat.directionSwitch = ctlStaInfo.pointSwitch;
        boardStat.targetType = ctlStaInfo.targetType;
    }

    if(osd_rect.detect.empty() == false){
        if(boardStat.targetLock == 0x11){
            boardStat.sysStat = 0x02;
        }else{
            // 锁定状态下目标丢失
            boardStat.sysStat = 0x04;
        }
        

        //映射到顺时针旋转90度的坐标系下（x',y'）=(h-y,x);
        int tx = osd_rect.detect[0].x;
        osd_rect.detect[0].x = VI_INPUT_HEIGHT - osd_rect.detect[0].y;
        osd_rect.detect[0].y = tx;

        boardStat.Pitchlos = osd_rect.detect[0].y;
        boardStat.Yawlos = osd_rect.detect[0].x;

        boardStat.targetWidth   = osd_rect.detect[0].h;
        boardStat.targetHeight  = osd_rect.detect[0].w;

        // 偏航，俯仰
        boardStat.yawLineSightAngle = ((osd_rect.detect[0].x + boardStat.targetWidth/2)*1.0/VI_INPUT_HEIGHT*28.0-14.0)*1000;
        boardStat.pitchSightAngle =  ((osd_rect.detect[0].y + boardStat.targetHeight/2)*1.0/VI_INPUT_WIDTH*40-20.0)*1000; 
    }else{
        // 目标搜索中
        boardStat.sysStat = 0x00;
    }

    

    // 自检结果
    boardStat.selfTestStat = 0x11;

    //空中目标
   
    boardStat.targetModify = 0x00;
    //

   
    sysStatReport(boardStat);
    

    return 0;
 }


void protocolServer::boardStatCheck(BoradStatus& stat){
    uint8_t * stInfo = (uint8_t*)&stat;
    uint32_t checkSum = 0;
    for(int i(2);i<85;i++){
        checkSum += stInfo[i];
    }

    stat.checksum = checkSum &0xFF;
}

void protocolServer::convertToEL(BoradStatus& stat,uint8_t* elMem){
    int offset = 0;
    // 0x55aa
    // INFO_PRT("stat head %x, %x, %x\n",stat.fixFrameHead,stat.fixFrameHead &0xFF,(stat.fixFrameHead >> 8)&0xFF);
    // 帧头
    elMem[offset++] = (stat.fixFrameHead >> 8)&0xFF;
    elMem[offset++] = stat.fixFrameHead &0xFF;

    // 帧长
    elMem[offset++] = stat.frameLength;

    // frame id
    elMem[offset++] = stat.frameID&0xFF;
    elMem[offset++] = (stat.frameID >> 8)&0xFF;

    elMem[offset++] = stat.selfTestStat;
    elMem[offset++] = stat.sysStat;
    elMem[offset++] = stat.xIndex;

    // 俯仰视线角
    elMem[offset++] = stat.pitchSightAngle & 0xFF;
    elMem[offset++] = (stat.pitchSightAngle >> 8)&0xFF;

    // 偏航视线角
    elMem[offset++] = stat.yawLineSightAngle & 0xFF;
    elMem[offset++] = (stat.yawLineSightAngle >> 8)&0xFF;
    
    // 发射标志
    elMem[offset++] = stat.launchFlag;

    // 目标类型
    elMem[offset++] = stat.targetType;

    // 目标锁定
    elMem[offset++] = stat.targetLock;

    // 目标切换
    elMem[offset++] = stat.targetModify;

    // 检测模式
    elMem[offset++] = stat.detectMode;

    // 俯仰
    elMem[offset++] = stat.Pitchlos&0xFF;
    elMem[offset++] = (stat.Pitchlos >> 8)&0xFF;
    // 偏航
    elMem[offset++] = stat.Yawlos&0xFF;
    elMem[offset++] = (stat.Yawlos >> 8)&0xFF;
    
    // 目标width
    elMem[offset++] = stat.targetWidth;
    // 目标height
    elMem[offset++] = stat.targetHeight;

    // 接收指向开关
    elMem[offset++] = stat.directionSwitch;

    // 下发俯仰视线角
    elMem[offset++] = stat.sendpitchSightAngle & 0xFF;
    elMem[offset++] = (stat.sendpitchSightAngle >> 8)&0xFF;

    // 下发偏航视线角
    elMem[offset++] = stat.sendyawLineSightAngle & 0xFF;
    elMem[offset++] = (stat.sendyawLineSightAngle >> 8)&0xFF;
    
    // 出视场
    elMem[offset++] = stat.outofView;

    // 保留
    memcpy(elMem + offset,stat.reverse,8);
    offset +=8;

    // 遥测数据
    memcpy(elMem + offset,stat.telemetryData,48);
    offset += 48;

    // 校验和
    elMem[offset++] = stat.checksum;

    // 帧尾
    elMem[offset++] = (stat.fixFrameTail >> 8)&0xFF;
    elMem[offset++] = stat.fixFrameTail &0xFF;

    // printf("\n\n");
    // for(int i(0);i<30;i++){
    //     printf("%x, ",elMem[i]);
    // }
    // printf("\n\n");
}

 int protocolServer::sysStatReport(BoradStatus& stat){
    // 消息校验
    boardStatCheck(stat);
    // 转换为小端字节序发送
    uint8_t elStr[sizeof(BoradStatus)] = {};
    convertToEL(stat,elStr);
    

    if(comTool.get()){
        comTool->serialSend((char*)elStr,sizeof(BoradStatus));
    }

    if(canTool.get()){
        canTool->canbusSend(elStr,sizeof(BoradStatus));
    }

    return 0;
 }